/**
 ****************************************************************************************************
 * @file        foc_motor.c
 * @author      哔哩哔哩-Rebron大侠
 * @version     V0.0
 * @date        2025-01-11
 * @brief       FOC电机状态机
 * @license     MIT License
 *              Copyright (c) 2025 Reborn大侠
 *              允许任何人使用、复制、修改和分发该代码，但需保留此版权声明。
 ****************************************************************************************************
 */

#include "foc_motor.h"
#include <foc_math.h>
#include <stdio.h>

/*电机初始化*/
void FOC_Motor_Init(FOC_MOTOR_t *foc_motor)
{
    foc_motor->motor_state = MOTOR_IDLE;

}

/*电机foc参数清除*/
static void FOC_Motor_Clear(FOC_MOTOR_t *foc_motor)
{
    abc_t abc_NULL  = {0};
    qd_t qd_NULL    = {0};
    alphabeta_t alphabeta_NULL = {0};

    /*FOC参数清除*/
    foc_motor->foc_var.I_abc = abc_NULL;
    foc_motor->foc_var.I_qd  = qd_NULL;
    foc_motor->foc_var.U_qd  = qd_NULL;
    foc_motor->foc_var.I_alphabeta = alphabeta_NULL;
    
    foc_motor->foc_var.I_qd_ref = qd_NULL;

    /*关闭PWM*/
    FOC_Port_Pwm_Stop();
}

/*电机下个状态*/
static void Motor_NextState(FOC_MOTOR_t *foc_motor, MOTOR_State_t new_state)
{
    foc_motor->motor_state = new_state;
}

/*电机开环运行*/
void Motor_Run_Open(FOC_MOTOR_t *foc_motor)
{
    FOC_VF_t *vf = &foc_motor->sensorless.vf;

    vf->Uqd.q = 1;
    FOC_VF_Angle_Calc(vf);

    float ElAngle = vf->speed.ElAngle;

    
    qd_t In = {0};
    /*更新电流*/
    
    foc_motor->foc_var.I_abc = (foc_motor->current.I_abc);
    /*克拉克变换*/
    foc_motor->foc_var.I_alphabeta  = FOC_Clarke(foc_motor->foc_var.I_abc);

    In = Foc_Park(foc_motor->foc_var.I_alphabeta, ElAngle);

    FOC_PWM_Run(&foc_motor->pwm, vf->Uqd, ElAngle);

    foc_motor->foc_var.I_qd = In;

}

/*电机无感运行*/
void Motor_SL_RUN(FOC_MOTOR_t *foc_motor)
{
    FOC_SENSORLESS_t *foc_sl = &foc_motor->sensorless;

    qd_t In = {0};
    qd_t Out = {0};
    abc_t Iabc = {0};
    float ElAngle = 0.0f;
    alphabeta_t I_alphabeta = {0};
    alphabeta_t Last_Ualphabeta = foc_motor->pwm.alpha_beta;

    /*更新电流*/
    Iabc = foc_motor->current.I_abc;
    /*克拉克变换*/
    I_alphabeta  = FOC_Clarke(Iabc);

    /*进入无感观测*/
    I_alphabeta = FOC_SL_RUN(&foc_motor->sensorless, I_alphabeta, Last_Ualphabeta);
    ElAngle = foc_motor->sensorless.speed.ElAngle;

    /*帕克变换*/
    In = Foc_Park(I_alphabeta, ElAngle);

    /*速度环控制*/    
    foc_motor->pid_cur_iq.SetPoint = foc_motor->foc_var.I_qd_ref.q;
    foc_motor->pid_cur_id.SetPoint = foc_motor->foc_var.I_qd_ref.d;

    Out.q = PID_Position_ctrl(&foc_motor->pid_cur_iq,  In.q);
    Out.d = PID_Position_ctrl(&foc_motor->pid_cur_id,  In.d) + foc_sl->d_bias;

    switch(foc_sl->sta){
        case VF_STA:
            PID_Clear(&foc_motor->pid_cur_iq);
            PID_Clear(&foc_motor->pid_cur_id);
            PID_Clear(&foc_motor->pid_speed);
            Out = foc_sl->vf.Uqd;
            break;
        case HFI_POL_STA:
            PID_Clear(&foc_motor->pid_cur_iq);
            PID_Clear(&foc_motor->pid_cur_id);
            PID_Clear(&foc_motor->pid_speed);
            Out.d = foc_sl->d_bias; 
            Out.q = 0;
            break;
        case HFI_STA:

            /*高频注入的时候不要对d轴电流进行PID控制，否则堵转下容易反转 并且要及时清除d轴PID参数，不然跳转到SMO会出问题*/
            PID_Clear(&foc_motor->pid_cur_id);
            Out.d = foc_sl->d_bias;      
            //Out.q =  foc_sl->vf.Uqd.q;
            foc_motor->pid_cur_iq.ActualValue_limit = foc_motor->pid_qd_limit_hif;         /*高频注入状态 需要限制电流环*/ 
            break;

        case SMO_STA:
            foc_motor->pid_cur_iq.ActualValue_limit = foc_motor->pid_qd_limit;
            break;
    }

    FOC_PWM_Run(&foc_motor->pwm, Out, ElAngle);

    /*保存参数*/
    foc_motor->foc_var.speed = foc_motor->sensorless.speed;
    // foc_motor->foc_var.speed = foc_motor->enc.speed;
    foc_motor->foc_var.I_abc = FOC_Rev_Clarke(I_alphabeta);
    foc_motor->foc_var.U_qd = foc_motor->pwm.Uqd;
    foc_motor->foc_var.I_qd = In;
    foc_motor->foc_var.I_alphabeta = I_alphabeta;
}

/*电机霍尔运行*/
void Motor_HALL_RUN(FOC_MOTOR_t *foc_motor)
{
    qd_t In = {0};
    qd_t Out = {0};
    abc_t Iabc = {0};
    float ElAngle = 0.0f;
    alphabeta_t I_alphabeta = {0};
    /*速度环控制*/
    foc_motor->pid_cur_iq.SetPoint = foc_motor->foc_var.I_qd_ref.q;
    foc_motor->pid_cur_id.SetPoint = foc_motor->foc_var.I_qd_ref.d;

    /*更新电流*/
    Iabc = foc_motor->current.I_abc;
    /*克拉克变换*/
    I_alphabeta  = FOC_Clarke(Iabc);
             
    ElAngle = FOC_HALL_Angle_Calc(&foc_motor->hall);

    //FOC_SMO_Angle_Calc(smo, I_alphabeta, Last_Ualphabeta); 

    /*帕克变换*/
    In = Foc_Park(I_alphabeta, ElAngle);
    Out.q = PID_Position_ctrl(&foc_motor->pid_cur_iq,  In.q);
    Out.d = PID_Position_ctrl(&foc_motor->pid_cur_id,  In.d);

   // Out = foc_motor->sensorless.vf.Uqd;
   //Out.q = foc_motor->sensorless.vf.Uqd.q;
    FOC_PWM_Run(&foc_motor->pwm, Out, ElAngle);

    /*保存参数*/
    foc_motor->foc_var.speed = foc_motor->hall.speed;
    foc_motor->foc_var.I_abc = Iabc;
    foc_motor->foc_var.U_qd = foc_motor->pwm.Uqd;
    foc_motor->foc_var.I_qd = In;
    foc_motor->foc_var.I_alphabeta = I_alphabeta;
}

/*电机状态机运行*/
void FOC_Motor_Run(FOC_MOTOR_t *foc_motor)
{
	MOTOR_State_t state = foc_motor->motor_state;

    switch(state){
        case MOTOR_IDLE:  /*空闲状态*/
            Motor_NextState(foc_motor, MOTOR_IDLE_START);             /*跳转到空闲启动*/
            printf("IDLE\r\n");    
            break;
        case MOTOR_IDLE_START:    
            if(0==FOC_Current_Calib(&foc_motor->current)){
                Motor_NextState(foc_motor, MOTOR_CLEAR);        /*跳转到电容充电*/
                printf("IDLE_START\r\n"); 
            }           
            break;        
        case MOTOR_CLEAR:
            FOC_Motor_Clear(foc_motor);
            FOC_Port_Pwm_Start();                                               /*打开PWM*/
            Motor_NextState(foc_motor, MOTOR_START);                                  /*跳转到启动*/
            printf("CLEAR\r\n"); 
            break;

        case MOTOR_START:
            Motor_NextState(foc_motor, MOTOR_SL_RUN);                           /*跳转到开始启动*/
            printf("START\r\n"); 
            break;

        case MOTOR_OPEN_RUN:
            Motor_Run_Open(foc_motor);
            break;
        case MOTOR_SL_RUN:
            Motor_SL_RUN(foc_motor);
            break;
        case MOTOR_HALL_RUN:
            /*霍尔传感器运行*/
            Motor_HALL_RUN(foc_motor);
            break;
        default:
            break;
    }
    
}

/*电机速度计算
Ts: 该函数的调用周期: 单位s
*/
void FOC_Motor_Speed_Calc(FOC_MOTOR_t *foc_motor, float Ts)
{
    FOC_Sensorless_Speed_Cale(&foc_motor->sensorless, Ts);                   /*无感速度计算*/

    FOC_Hall_Speed_Calc(&foc_motor->hall);                                  /*霍尔传感器计算速度*/
}

/*速度环控制, 返回参考目标力矩Iq*/
void FOC_Motor_Speed_Ctrl(FOC_MOTOR_t *foc_motor)
{

    foc_motor->pid_speed.SetPoint = foc_motor->target_speed;

    float speed = foc_motor->foc_var.speed.AvrMecSpeed;

    if(fabsf(speed)>15000){
        foc_motor->target_speed  = -foc_motor->target_speed;
    }

    foc_motor->foc_var.I_qd_ref.q = PID_Position_ctrl(&foc_motor->pid_speed, speed);
    foc_motor->foc_var.I_qd_ref.d = 0.0001f;       /*不能设置为0, 为了避免pid函数里进入退饱和*/

}

/*电机堵转的操作 自由开发*/
void FOC_Motor_Lock(FOC_MOTOR_t *foc_motor)
{
    /*清除PID参数*/
    PID_Clear(&foc_motor->pid_cur_iq);
    PID_Clear(&foc_motor->pid_cur_id);
    PID_Clear(&foc_motor->pid_speed);
}
